#ifndef RANDOMWALK_H_
#define RANDOMWALK_H_

#include "../Driver.h"
#include <std_msgs/Float32MultiArray.h>
#include <boost/thread.hpp>

using namespace std;
namespace std {
using boost::mutex;
using boost::condition_variable;
using boost::unique_lock;
using boost::thread;
}

#define MAX_RANGE_SIZE 720 // maximal size for laser scan data

/**
 * Class for random moving when object is not visible
 */
class RandomWalk {

private:

	// ROS management objects
	ros::NodeHandle m_node_handler;
	ros::Rate* m_loop_rate;
	ros::Publisher m_sensors_publisher;
	ros::Subscriber sub_sensors_fl;

	Driver* m_driver; // object to move the robot
	float m_speed; // linear driving speed (meter/second)

	// used for synchronized access to data
	std::mutex m_mutex;

	// sections of front laser's range (used to detect obstacles)
	float m_east_bin;	
	float m_north_east_bin;	
	float m_north_bin;	
	float m_north_west_bin;	
	float m_west_bin;

	float m_range[MAX_RANGE_SIZE]; // current sample from front laser

	short m_read_sensor; // if not 0, then should consider obstacles indicated by laser sensor

	/**
	 * Subscribes to laser for the emergency stop checks
	 * @param shouldSubscribe true to subscribe, false to cancel subscription
	 */
	void subscribeToLaser(bool shouldSubscribe);

	/**
	 * Calculates average of values in array in given interval
	 * @param range array of values, as sampled from front laser sensor
	 * @param start start of interval
	 * @param end end of interval
	 * @return average of values in input array in given interval
	 */
	float avgElements(const std_msgs::Float32MultiArray::ConstPtr& range, const int& start, const int& end) const;

	/**
	 * Calculates average of values in array in given interval
	 * @param range array of values
	 * @param start start of interval
	 * @param end end of interval
	 * @return average of values in input array in given interval
	 */
	float avgElements(const float range[], const int& start, const int& end) const;

	/**
	 * Safely converts RosMessage multi-array to float array. \n
	 * Updates member m_range.
	 * @param range array of distances from laser
	 */
	void setRange(const std_msgs::Float32MultiArray::ConstPtr& range);

	/**
	 * Reads safely (with mutex) the distances (in meters) from laser of each side. \n
	 * Each distance is a member that is updated in callback from \ref SensorNode
	 * @param[out] east average value of east side of laser range
	 * @param[out] west average value of west side of laser range
	 */
	void getBins(float& east,float& west);

	/**
	 * Reads safely (with mutex) the distances (in meters) from laser of each side. \n
	 * Each distance is a member that is updated in callback from \ref SensorNode
	 * @param[out] north_east average value of north_east side of laser range
	 * @param[out] north average value of north side of laser range
	 * @param[out] north_west average value of north_west side of laser range
	 */
	void getBins(float& north_east,float& north, float& north_west);

	/**
	 * Reads safely (with mutex) the distances (in meters) from laser of each side. \n
	 * Each distance is a member that is updated in callback from \ref SensorNode
	 * @param[out] east average value of east side of laser range
	 * @param[out] north_east average value of north_east side of laser range
	 * @param[out] north average value of north side of laser range
	 * @param[out] north_west average value of north_west side of laser range
	 * @param[out] west average value of west side of laser range
	 */
	void getBins(float& east,float& north_east,float& north, float& north_west, float& west);

	/**
	 * Updates members that stores distances from robot
 	 * @param[in] east average value of east side of laser range
	 * @param[in] north_east average value of north_east side of laser range
	 * @param[in] north average value of north side of laser range
	 * @param[in] north_west average value of north_west side of laser range
	 * @param[in] west average value of west side of laser range
	 */
	void setBins(const float east,const float north_east,const float north, const float north_west, const float west);

	/**
	 * Randomly choose sign of turn
	 * @return \ref RIGHT or \ref LEFT
	 */
	short chooseSign();

	/**
	 * If an obstacle is too close to front laser's left, right or middle
	 * area (average value in one of these bins is too small),
	 * then the robot is too close to an obstacle and should stop
	 * @return True if should perform emegency stop, False if not
	 */
	bool isEmergencyStop();

	/**
	 * Stops code execution for \ref TIME_FOR_TURN seconds.
	 * Works like regular sleep, but every TIME_FOR_TURN/1000 seconds runs ros::spinOnce
	 * to refresh data from callbacks
	 */
	void sleepAndWatch();

public:

	/**
	 * Constructor- initializes the walker. \n
	 * Sets default parameters and subscribes to the laser.
	 * Also creates publisher to subscribe to \ref SensorNode.
	 */
	RandomWalk(Driver* driver, const float& speed);

	/**
	 * Destructor- virtual, to allow inheritance
	 */
	virtual ~RandomWalk();

	/**
	 * Callback for front laser's range. \n
	 * In this topic the SensorNode will publish distances from the laser.
	 * @param range RosMessage array of distances from laser
	 */
	void frontLaserCB(const std_msgs::Float32MultiArray::ConstPtr& range);

	/**
	 * Randomly choose direction to turn and turn when approaching to obstacle
	 */
	void chooseDirection();
};

#endif /* RANDOMWALK_H_ */
